clear
clc

number=1000;
%dg
dg_min=0;
dg_max=10;
ddg=rand(number,1);
dg=dg_min+(dg_max-dg_min)*ddg;
dg=[dg;dg_min;dg_max];
dg_mean=mean(dg);
dg_std=std(dg);
ug1=exp(-power(dg-0,2)/power(dg_std,2));%near
ug2=exp(-power(dg-10,2)/power(dg_std,2));%far
%do
do_min=0;
do_max=2;
ddo=rand(number,1);
do=do_min+(do_max-do_min)*ddo;
do=[do;do_min;do_max];
do_mean=mean(do);
do_std=std(do);
uo1=exp(-power(do-0,2)/power(do_std,2));
uo2=exp(-power(do-2,2)/power(do_std,2));
%sig
sig_min=-pi/2;
sig_max=pi/2;
dsig=rand(number,1);
sig=sig_min+(sig_max-sig_min)*dsig;
sig=[sig;sig_min;sig_max];
sig_mean=mean(sig);
sig_std=std(sig);
usg1=exp(-power(sig-sig_min,2)/power(sig_std,2));%right
usg2=exp(-power(sig-sig_max,2)/power(sig_std,2));%left
%sio
sio_min=-pi/2;
sio_max=pi/2;
dsio=rand(number,1);
sio=sio_min+(sio_max-sio_min)*dsio;
sio=[sio;sio_min;sio_max];
sio_mean=mean(sio);
sio_std=std(sio);
uso1=exp(-power(sio-sio_min,2)/power(sio_std,2));%right
uso2=exp(-power(sio-sio_max,2)/power(sio_std,2));%left
%output
outspeed=[];
for i=1:number+2
    ug1i=ug1(i);
    ug2i=ug2(i);
    if(ug1i>=ug2i)
        ug="near";
    else
        ug="far";
    end
    uo1i=uo1(i);
    uo2i=uo2(i);
    if(uo1i>=uo2i)
        uo="near";
    else
        uo="far";
    end
    usg1i=usg1(i);
    usg2i=usg2(i);
    if(usg1i>=usg2i)
        usg="right";
    else
        usg="left";
    end
    uso1i=uso1(i);
    uso2i=uso2(i);
    if(uso1i>=uso2i)
        uso="right";
    else
        uso="left";
    end
    %
    if(ug=="near"&&uo=="near"&&usg=="left"&&uso=="left")
        v=0.69;
        w=-1.63;
    elseif(ug=="near"&&uo=="near"&&usg=="left"&&uso=="right")
        v=0.69;
        w=1.63;
    elseif(ug=="near"&&uo=="near"&&usg=="right"&&uso=="right")
        v=0.69;
        w=1.63;
    elseif(ug=="near"&&uo=="near"&&usg=="right"&&uso=="left")
        v=0.69;
        w=-1.63;
        %
    elseif(ug=="near"&&uo=="far"&&usg=="left"&&uso=="left")
        v=0.69;
        w=1.63;
    elseif(ug=="near"&&uo=="far"&&usg=="left"&&uso=="right")
        v=0.69;
        w=1.63;
    elseif(ug=="near"&&uo=="far"&&usg=="right"&&uso=="right")
        v=0.69;
        w=-1.63;
    elseif(ug=="near"&&uo=="far"&&usg=="right"&&uso=="left")
        v=0.69;
        w=-1.63;
        %
    elseif(ug=="far"&&uo=="far"&&usg=="left"&&uso=="left")
        v=1.01;
        w=1.58;
    elseif(ug=="far"&&uo=="far"&&usg=="left"&&uso=="right")
        v=1.01;
        w=1.58;
    elseif(ug=="far"&&uo=="far"&&usg=="right"&&uso=="right")
        v=1.01;
        w=-1.58;
    elseif(ug=="far"&&uo=="far"&&usg=="right"&&uso=="left")
        v=1.01;
        w=-1.58;
        %
    elseif(ug=="far"&&uo=="near"&&usg=="left"&&uso=="left")
        v=0.69;
        w=-1.63;
    elseif(ug=="far"&&uo=="near"&&usg=="left"&&uso=="right")
        v=0.69;
        w=1.63;
    elseif(ug=="far"&&uo=="near"&&usg=="right"&&uso=="right")
        v=0.69;
        w=1.63;
    elseif(ug=="far"&&uo=="near"&&usg=="right"&&uso=="left")
        v=0.69;
        w=-1.63;
    end
    if(do(i)<0.15||dg(i)<0.15)
        v=0;
        w=0;
    end
    if(sig(i)<pi/180&&sig(i)>-pi/180)
        w=0;
    end
    outspeed=[outspeed;ug,uo,usg,uso,v,w];
end
trainSet=[dg,do,sig,sio,outspeed];

n=number/2;
rowIndices=randperm(number,n);

trainSet1=trainSet(rowIndices,:);

testSet=trainSet(setdiff(1:number,rowIndices),:);